/*=====================================================================*
 *                   Copyright (C) 2011 Paul Mineiro                   *
 * All rights reserved.                                                *
 *                                                                     *
 * Redistribution and use in source and binary forms, with             *
 * or without modification, are permitted provided that the            *
 * following conditions are met:                                       *
 *                                                                     *
 *     * Redistributions of source code must retain the                *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer.                                       *
 *                                                                     *
 *     * Redistributions in binary form must reproduce the             *
 *     above copyright notice, this list of conditions and             *
 *     the following disclaimer in the documentation and/or            *
 *     other materials provided with the distribution.                 *
 *                                                                     *
 *     * Neither the name of Paul Mineiro nor the names                *
 *     of other contributors may be used to endorse or promote         *
 *     products derived from this software without specific            *
 *     prior written permission.                                       *
 *                                                                     *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND              *
 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,         *
 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES               *
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE             *
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER               *
 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,                 *
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES            *
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE           *
 * GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR                *
 * BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF          *
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT           *
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY              *
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE             *
 * POSSIBILITY OF SUCH DAMAGE.                                         *
 *                                                                     *
 * Contact: Paul Mineiro <paul@mineiro.com>                            *
 *=====================================================================*/

#ifndef __NEWTON_HH__
#define __NEWTON_HH__

#include <cmath>

namespace playerpiano
{

template<typename F>
float
newton (const F&        f,
        float&          x,
        unsigned int    maxeval,
        float           tol)
{
  float dfdx;
  float ddfddx;
  float fx = f (x, dfdx, ddfddx);
  static const float epsilon = 1e-4;
  unsigned int eval = 1;

  while (fabs (ddfddx) > epsilon && eval < maxeval)
    {
      float step = -dfdx / fabs (ddfddx);

      if (fabs (step) > 1)
        {
          step = (step > 0) ? 1 : -1;
        }

      if (fabs (step) < tol)
        {
          break;
        }

      float newx = x - step;
      float fxnew = f (newx, dfdx, ddfddx);
      ++eval;

      while (fxnew < fx && fabs (step) > tol && eval < maxeval)
        {
          step *= 0.5;
          newx = x - step;
          fxnew = f (newx, dfdx, ddfddx);
          ++eval;
        }

      if (fabs (step) < tol || eval >= maxeval)
        {
          break;
        }

      x = newx;
      fx = fxnew;
    }

  return fx;
}

}
#endif /* __NEWTON_HH__ */
